#include"visual_odom/landmark.h"

namespace test_visual_odom{

LandMark::Ptr LandMark::createNewLandmark() {
    static long factory_id = 0;
    LandMark::Ptr new_landmark(new LandMark);
    new_landmark->id_ = factory_id++;
    return new_landmark;
}

void LandMark::removeFeaturePoint2D(shared_ptr<FeaturePoint> feat) {
    unique_lock<mutex> lck(data_mutex_);
    for (auto iter = feature_points2D_.begin(); iter != feature_points2D_.end();
         iter++) {
        if (iter->lock() == feat) {
            feature_points2D_.erase(iter);
            feat->p_landmark_.reset();
            observed_times_--;
            break;
        }
    }
}

}